//
// Created by Yuxuan Han on 10/19/2018.
//

#include <stdio.h>
#include <math.h>


struct position{
    float x; //x-coordinate
    float y; //y-coordinate
    int angle; //0-360
    float length; //for target_pos specifically
};

struct position current_pos; //implement current position structure
struct position target_pos; //implement target position structure

//struct position *current_pos_ptr=&current_pos;


void set_ini_pos(struct position *pos_obj, float x_offset, float y_offset){ //initial value
    pos_obj->x=x_offset;
    pos_obj->y=y_offset;
}

void set_ini_angle(struct position *pos_obj,int angle){
    pos_obj->angle=angle;
}

void set_tar_angle(struct position *pos_obj,int angle){
    pos_obj->angle=angle;
}

void set_tar_pos(struct position *pos_obj, float x_tar, float y_tar) { //set tar;
    pos_obj->x=x_tar;
    pos_obj->y=y_tar;
}

/* The reason I calculate length and angle separately is.... I don't know why */

float length_cal(float x1, float y1, float x2, float y2) {
    float length=sqrtf((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
    return length;
}

int angle_cal(){
    float angle_in_rad;
    float length;

    length=length_cal(current_pos.x,current_pos.y,target_pos.x,target_pos.y);
    angle_in_rad=acosf((target_pos.x-current_pos.x)/length);
    angle_in_rad*=(180/3.1415926); //rad->degree

    return (int)angle_in_rad;
}


/*struct pid_controller{
    float kp,ki,kd;
    int target,current;
    int error;
    int integral;
    int last_error;
};


struct pid_controller controller1;



void pid_set_param(struct pid_controller *pid_obj, float kp, float ki, float kd){
    pid_obj->kp=kp;
    pid_obj->ki=ki;
    pid_obj->kd=kd;
}

void pid_init(struct pid_controller *pid_obj){
    pid_obj->integral=0;
    pid_obj->last_error=0;
}

int pid_cal(struct pid_controller *pid_obj){
    float output;
    pid_obj->error=pid_obj->target-pid_obj->current;
    pid_obj->integral+=pid_obj->error;
    output=pid_obj->kp*pid_obj->error+pid_obj->ki*pid_obj->integral+pid_obj->kd*(pid_obj->error-pid_obj->last_error);
    pid_obj->last_error=pid_obj->error; //update error
    return (int)output;
}*/
